// 2022-5-30 0.0.0.6A

#ifndef PATH_FOLLOWER
#define PATH_FOLLOWER

#include <string>
#include <vector>
#include <math.h>
#include <ros/ros.h>
#include <limits.h>
#include <vector>
#include <tf/tf.h>
#include <angles/angles.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/TwistWithCovarianceStamped.h>
#include <tecbot_msgs/obstacle.h>
#include "navigation/utils/controllers.h"
#include "navigation/navigation_state_code.h"
#include "navigation/utils/local_planner/mover.h"

#define PF_DEBUG 1

using namespace std;
using namespace navigation_ns;

class path_follower
{
public:
    path_follower(float max_x_vel, float min_x_vel, float max_x_acc, float max_yaw_vel, float min_yaw_vel, float max_yaw_acc, bool allow_reverse_drive,
                  bool use_advanced_evaluation, bool use_kinematic_limits, bool use_find_mode_follower,
                  float velocity_dec_offset_time,
                  float find_mode_x_vel, float find_mode_yaw_vel, float find_mode_radius, float find_mode_min_dist_timeout, bool enable_near_angle_correct,
                  float angle_diff_to_forward, float angle_diff_to_rotate,
                  float xy_tolerance, float yaw_tolerance,
                  float chassis_radius,
                  float sim_time, int sim_vx_space_size, int sim_vyaw_space_size, int trajectory_sim_step, float follow_point_weight, float follow_path_weight,
                  float stop_distance, float turning_stop_distance, float ignore_obstacle_dist,
                  float obstacle_acc_ratio,
                  float control_freq);
    ~path_follower();
    void set_kinematic_params(float max_x_vel, float max_yaw_vel);
    bool make_plan(geometry_msgs::PoseWithCovarianceStamped pose, vector<geometry_msgs::PoseStamped> plan, bool ignore_obstacle);
    bool start_session();
    bool pause_session();
    bool continue_session();
    navigation_state loop(geometry_msgs::PoseWithCovarianceStamped pose,
                          geometry_msgs::TwistWithCovarianceStamped odom_twist,
                          tecbot_msgs::obstacle obstacle);
    geometry_msgs::Twist get_current_twist();
    geometry_msgs::PoseWithCovarianceStamped get_current_pose();
    geometry_msgs::PoseStamped get_current_path_point_pose();
    block_state get_obstacle_state();
    void reset_state();

private:
    float max_x_vel_, min_x_vel_, max_x_acc_, max_yaw_vel_, min_yaw_vel_, max_yaw_acc_;
    bool allow_reverse_drive_;
    bool use_advanced_evaluation_;
    bool use_kinematic_limits_;
    bool use_find_mode_follower_;

    float velocity_dec_offset_time_;

    float find_mode_x_vel_, find_mode_yaw_vel_, find_mode_radius_, find_mode_min_dist_timeout_;
    bool enable_near_angle_correct_;
    float angle_diff_to_forward_, angle_diff_to_rotate_;
    float xy_tolerance_, yaw_tolerance_;
    float chassis_radius_;
    float sim_time_;
    int sim_vx_space_size_, sim_vyaw_space_size_, trajectory_sim_step_;
    float follow_point_weight_, follow_path_weight_;
    float stop_distance_, turning_stop_distance_, ignore_obstacle_dist_;

    float obstacle_acc_ratio_;

    float control_freq_;

    geometry_msgs::Pose goal_;
    vector<geometry_msgs::PoseStamped> plan_;
    bool ignore_obstacle_;

    geometry_msgs::PoseWithCovarianceStamped pose_;
    tecbot_msgs::obstacle obstacle_;
    int current_point_in_path_;

    unique_ptr<mover> mover_;

    bool reverse_drive_;
    bool find_mode_;
    int find_mode_counter_;
    float dist_;
    float min_dist_;
    bool near_angle_corrected_;
    bool stand_at_goal_;

    bool acc_controlled_;
    bool last_acc_controlled_;
    geometry_msgs::Twist twist_target_;
    geometry_msgs::Twist last_twist_target_;
    geometry_msgs::Twist twist_;
    geometry_msgs::TwistWithCovarianceStamped odom_twist_;
    float vx_delta_;
    float vyaw_delta_;

    block_state obstalce_state_;
    navigation_state state_;
    navigation_state last_state_before_blocked_;
    navigation_state last_state_;

    ros::NodeHandle node_handle_;
    ros::Publisher current_point_in_path_pub_;
    ros::Publisher point_to_track_pub_;
    ros::Publisher all_trajectory_pub_;
    ros::Publisher best_trajectory_pub_;

    nav_msgs::Path current_best_trajectory_;

    enum rotation_state
    {
        ready,
        running,
        reached
    };

    rotation_state rotation_state_;

    void update_current_point_in_path();
    bool rotate(float angle = 0.0f, float tolerance = 0.0f, float max_yaw_vel = 0.0f);
    int get_max_sim_distance_point_index();
    int get_max_sim_angle_point_index(int point_index);
    float calc_goal_angle();
    float calc_global_plan_angle(int point_index);
    void trajectory_simulation(float &vx, float &vyaw,
                               int max_sim_point_index,
                               float max_x_vel, float max_yaw_vel);
    block_state is_obstacle_detected();
    float distance(geometry_msgs::Pose pose0, geometry_msgs::Pose pose1);
};

#endif